import xml.etree.ElementTree as ET
import argparse, os
from datetime import datetime

parser = argparse.ArgumentParser(description='转换 AGVParam XML 文件支持ROS2')
parser.add_argument('file_path', type=str, help='XML文件的完整路径')
args = parser.parse_args()



if os.path.exists(args.file_path):
    file_path = args.file_path
    directory, filename = os.path.split(file_path)  
    file_name, _ = os.path.splitext(filename) 

    current_time = datetime.now().strftime("%Y%m%d_%H%M%S") 
    new_filename = f"{file_name}_{current_time}.xml"
    new_file_path = os.path.join(directory, new_filename)
    
    is_update_needed = False

    print("当前文件:", args.file_path)
    tree = ET.parse(args.file_path) 
    root = tree.getroot()

    agv_type = root.find(".//AgvType[@TypeName='车辆类型']")
    if agv_type is not None:
        agv_param = agv_type.find("./Component[@SymbolName='AGVParam']")
        if agv_param is not None:
            version_component = agv_param.find(".//Component[@SymbolName='version']")
            if version_component is not None:
                # 获取 value 值并判断
                value = float(version_component.get('Value', 0))
                if value < 2.0:
                    print(f" 当前版本号为 {value}，需要更新。")
                    is_update_needed = True
                else:
                    print(f" 当前版本号为 {value}，不需要更新。")
            else:
                new_version = ET.Element('Component', {
                    'DataType': 'float',
                    'Remark': '参数版本',
                    'Max': '',
                    'SymbolName': 'version',
                    'OlineRW': 'LocalOnly',
                    'Unit': '',
                    'Value': '2.0',
                    'Min': ''
                })
                is_update_needed = True
                agv_param.append(new_version)
                print(f" 更新版本-2.0")
        controllers_component = agv_type.find(".//Component[@SymbolName='controllers']")
        if controllers_component is not None:
            controller_name_component = controllers_component.findall(".//Component[@SymbolName='controller_name']")
            controller_names = [cn.get('Value') for cn in controller_name_component]

            for cn in controller_name_component:
                current_value = cn.get('Value')
                if current_value == "chassis_controller":
                    cn.set('Value', "chassis_controller/ChassisController")
                    is_update_needed = True
                    print(f" {current_value} 更新为 {cn.get('Value')}")
                elif current_value == "position_controller":
                    cn.set('Value', "position_controller/JointGroupPositionController")
                    is_update_needed = True
                    print(f" {current_value} 更新为 {cn.get('Value')}")
                elif current_value == "velocity_controller":
                    cn.set('Value', "velocity_controller/JointGroupVelocityController")
                    is_update_needed = True
                    print(f" {current_value} 更新为 {cn.get('Value')}")
            
            if "joint_state_broadcaster/JointStateBroadcaster" not in controller_names:
                # 插入 joint_state_broadcaster 组件
                new_controller = ET.Element('Component', {
                    'DataType': '',
                    'Remark': 'controller',
                    'Max': '',
                    'SymbolName': 'joint_state_broadcaster',
                    'OlineRW': 'LocalOnly',
                    'Unit': '',
                    'Value': '',
                    'Min': ''
                })
                controller_name = ET.Element('Component', {
                    'DataType': 'str',
                    'Remark': 'controller',
                    'Max': '',
                    'SymbolName': 'controller_name',
                    'OlineRW': 'LocalOnly',
                    'Unit': '',
                    'Value': 'joint_state_broadcaster/JointStateBroadcaster',
                    'Min': ''
                })
                new_controller.append(controller_name)
                controllers_component.append(new_controller)
                is_update_needed = True
                print(f" 插入joint_state_broadcaster类型")

            if "update_rate" not in controller_names:
                # 插入 update_rate 组件
                update_rate = ET.Element('Component', {
                    'DataType': 'int',
                    'Remark': '更新频率',
                    'Max': '200',
                    'SymbolName': 'update_rate',
                    'OlineRW': 'LocalOnly',
                    'Unit': '',
                    'Value': '100',
                    'Min': '50'
                })
                controllers_component.append(update_rate)
                is_update_needed = True
                print(f" 插入update_rate类型")

        if is_update_needed:      
            try:
                os.rename(args.file_path, new_file_path)
            except FileNotFoundError:
                print(f"原文件不存在，请检查路径。")
            except Exception as e:
                print(f"重命名文件时发生错误：{e}")

            tree.write(args.file_path, encoding='utf-8', xml_declaration=True)
        print(f"参数转换完成。") 
else:
    print(f"文件 {args.file_path} 不存在。")


